
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_position_control.h
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <pid/p.h>            // P library
#include <pid/pid.h>          // PID library
#include <pid/p_1d.h>         // P library (1-axis)
#include <pid/p_2d.h>         // P library (2-axis)
#include <pid/pi_2d.h>        // PI library (2-axis)
#include <pid/pid_basic.h>    // PID library (1-axis)
#include <pid/pid_2d.h>       // PID library (2-axis)

#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
#include <motor/gp_motors.h>
#include <common/gp_defines.h>
#include <common/gp_math/gp_mathlib.h>
#include <common/gp_math/gp_control.h>
#include <mc_attitude_control/mc_attitude_control_multi.h>
/*-----------------------------------macro------------------------------------*/
// position controller default definitions
#define POSCONTROL_ACCEL_XY                     100.0f  // default horizontal acceleration in cm/s/s.  This is overwritten by waypoint and loiter controllers
#define POSCONTROL_JERK_XY                      5.0f    // default horizontal jerk m/s/s/s

#define POSCONTROL_STOPPING_DIST_UP_MAX         300.0f  // max stopping distance (in cm) vertically while climbing
#define POSCONTROL_STOPPING_DIST_DOWN_MAX       200.0f  // max stopping distance (in cm) vertically while descending

#define POSCONTROL_SPEED                        500.0f  // default horizontal speed in cm/s
#define POSCONTROL_SPEED_DOWN                  -150.0f  // default descent rate in cm/s
#define POSCONTROL_SPEED_UP                     250.0f  // default climb rate in cm/s

#define POSCONTROL_ACCEL_Z                      250.0f  // default vertical acceleration in cm/s/s.
#define POSCONTROL_JERK_Z                       5.0f    // default vertical jerk m/s/s/s

#define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ      2.0f    // low-pass filter on acceleration error (unit: Hz)

#define POSCONTROL_OVERSPEED_GAIN_Z             2.0f    // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range

#define POSCONTROL_RELAX_TC                     0.16f   // This is used to decay the relevant variable to 5% in half a second.
/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    float ahrsGndSpdLimit;
    float ahrsControlScaleXY;
    float controlScaleZ;
} PosCtrl_AttDataTypeDef;

// general purpose flags
typedef struct {
    uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
} poscontrol_flags;

// limit flags structure
typedef struct {
    bool pos_xy;        // true if we have hit a horizontal position limit
    bool pos_up;        // true if we have hit a vertical position limit while going up
    bool pos_down;      // true if we have hit a vertical position limit while going down
} poscontrol_limit_flags;

typedef struct
{
    PosCtrl_AttDataTypeDef _psc_data;

    poscontrol_flags _flags;
    poscontrol_limit_flags _limit;

    // references to inertial nav and ahrs libraries
    ahrs_view*         _ahrs;
    Motors_HandleTypeDef*        _motors;
    Attitude_ctrl*     _attitude_control;

    // parameters
    Param_float        _lean_angle_max;    // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
    Param_float        _shaping_jerk_xy;   // Jerk limit of the xy kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
    Param_float        _shaping_jerk_z;    // Jerk limit of the z kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
    P_2d_ctrl          _p_pos_xy;          // XY axis position controller to convert distance error to desired velocity
    P_1d_ctrl          _p_pos_z;           // Z axis position controller to convert altitude error to desired climb rate
    PID_2d_ctrl        _pid_vel_xy;        // XY axis velocity controller to convert velocity error to desired acceleration
    PID_basic_ctrl     _pid_vel_z;         // Z axis velocity controller to convert climb rate error to desired acceleration
    PID_ctrl           _pid_accel_z;       // Z axis acceleration controller to convert desired acceleration to throttle output

    // internal variables
    float       _dt;                    // time difference (in seconds) between calls from the main program
    uint64_t    _last_update_xy_us;     // system time (in microseconds) since last update_xy_controller call
    uint64_t    _last_update_z_us;      // system time (in microseconds) since last update_z_controller call
    float       _vel_max_xy_cms;        // max horizontal speed in cm/s used for kinematic shaping
    float       _vel_max_up_cms;        // max climb rate in cm/s used for kinematic shaping
    float       _vel_max_down_cms;      // max descent rate in cm/s used for kinematic shaping
    float       _accel_max_xy_cmss;     // max horizontal acceleration in cm/s/s used for kinematic shaping
    float       _accel_max_z_cmss;      // max vertical acceleration in cm/s/s used for kinematic shaping
    float       _jerk_max_xy_cmsss;           // Jerk limit of the xy kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
    float       _jerk_max_z_cmsss;            // Jerk limit of the z kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
    float       _vel_z_control_ratio;   // default 2.0f,confidence that we have control in the vertical axis

    // output from controller
    float       _roll_target;           // desired roll angle in centi-degrees calculated by position controller
    float       _pitch_target;          // desired roll pitch in centi-degrees calculated by position controller
    float       _yaw_target;            // desired yaw in centi-degrees calculated by position controller
    float       _yaw_rate_target;       // desired yaw rate in centi-degrees per second calculated by position controller

    // position controller internal variables
    Vector3p    _pos_target;            // target location in NEU cm from home
    Vector3f_t  _vel_desired;           // desired velocity in NEU cm/s
    Vector3f_t  _vel_target;            // velocity target in NEU cm/s calculated by pos_to_rate step
    Vector3f_t  _accel_desired;         // desired acceleration in NEU cm/s/s (feed forward)
    Vector3f_t  _accel_target;          // acceleration target in NEU cm/s/s
    Vector3f_t  _limit_vector;          // the direction that the position controller is limited, zero when not limited
    Vector2f_t  _vehicle_horiz_vel;     // velocity to use if _flags.vehicle_horiz_vel_override is set
    float       _pos_offset_z;          // vertical position offset in NEU cm from home
    float       _vel_offset_z;          // vertical velocity offset in NEU cm/s calculated by pos_to_rate step
    float       _accel_offset_z;        // vertical acceleration offset in NEU cm/s/s

    // ekf reset handling
    uint32_t    _ekf_xy_reset_ms;       // system time of last recorded ekf xy position reset
    uint32_t    _ekf_z_reset_ms;        // system time of last recorded ekf altitude reset

    // high vibration handling
    bool        _vibe_comp_enabled;     // true when high vibration compensation is on
} Position_ctrl;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void posctrl_ctor(Position_ctrl* pos_ctrl, ahrs_view* ahrs,Motors_HandleTypeDef* motors, Attitude_ctrl* attitude_control, float dt);

/// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
///     Position and velocity errors are converted to velocity and acceleration targets using PID objects
///     Desired velocity and accelerations are added to these corrections as they are calculated
///     Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
void posctrl_update_xy_controller(Position_ctrl* pos_ctrl);

/// get_dt - gets time delta in seconds for all position controllers
static inline float posctrl_get_dt(Position_ctrl* pos_ctrl) { return pos_ctrl->_dt; }

/// get_shaping_jerk_xy_cmsss - gets the jerk limit of the xy kinematic path generation in cm/s/s/s
static inline float posctrl_get_shaping_jerk_xy_cmsss(Position_ctrl* pos_ctrl) { return pos_ctrl->_shaping_jerk_xy*100.0; }

///
/// 3D position shaper
///

/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
void posctrl_input_pos_xyz(Position_ctrl* pos_ctrl, const Vector3p* pos, float pos_offset_z, float pos_offset_z_buffer);

/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
///     This function only needs to be called if using the kinematic shaping.
///     This can be done at any time as changes in these parameters are handled smoothly
///     by the kinematic shaping.
void posctrl_set_max_speed_accel_xy(Position_ctrl* pos_ctrl, float speed_cms, float accel_cmss);

/// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit
///     This should be done only during initialisation to avoid discontinuities
void posctrl_set_correction_speed_accel_xy(Position_ctrl* pos_ctrl, float speed_cms, float accel_cmss);

/// get_max_speed_xy_cms - get the maximum horizontal speed in cm/s
static inline float posctrl_get_max_speed_xy_cms(Position_ctrl* pos_ctrl) { return pos_ctrl->_vel_max_xy_cms; }

/// get_max_accel_xy_cmss - get the maximum horizontal acceleration in cm/s/s
static inline float posctrl_get_max_accel_xy_cmss(Position_ctrl* pos_ctrl) { return pos_ctrl->_accel_max_xy_cmss; }

// set the maximum horizontal position error that will be allowed in the horizontal plane
static inline void posctrl_set_pos_error_max_xy_cm(Position_ctrl* pos_ctrl, float error_max) { p_2d_ctrl_set_error_max(&pos_ctrl->_p_pos_xy, error_max); }
static inline float posctrl_get_pos_error_max_xy_cm(Position_ctrl* pos_ctrl) { return p_2d_ctrl_get_error_max(&pos_ctrl->_p_pos_xy); }

/// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
void posctrl_init_xy_controller(Position_ctrl* pos_ctrl);

/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
///     This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
///     The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void posctrl_init_xy_controller_stopping_point(Position_ctrl* pos_ctrl);

// relax_velocity_controller_xy - initialise the position controller to the current position and velocity with decaying acceleration.
///     This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void posctrl_relax_velocity_controller_xy(Position_ctrl* pos_ctrl);

/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
///     The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
///     The jerk limit also defines the time taken to achieve the maximum acceleration.
void posctrl_input_accel_xy(Position_ctrl* pos_ctrl, const Vector3f_t* accel);

/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
///     The vel is projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_vel_accel_xy(Position_ctrl* pos_ctrl, Vector2f_t* vel, const Vector2f_t* accel, bool limit_output);

/// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
///     The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The function alters the pos and vel to be the kinematic path based on accel
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_pos_vel_accel_xy(Position_ctrl* pos_ctrl, Vector2p* pos, Vector2f_t* vel, const Vector2f_t* accel, bool limit_output);

// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool posctrl_is_active_xy(Position_ctrl* pos_ctrl);

/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
void posctrl_stop_pos_xy_stabilisation(Position_ctrl* pos_ctrl);

/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
void posctrl_stop_vel_xy_stabilisation(Position_ctrl* pos_ctrl);

// get_lean_angles_to_accel - convert roll, pitch lean angles to NE frame accelerations in cm/s/s
void posctrl_accel_to_lean_angles(Position_ctrl* pos_ctrl, float accel_x_cmss, float accel_y_cmss, float* roll_target, float* pitch_target);

// calculate_yaw_and_rate_yaw - update the calculated the vehicle yaw and rate of yaw.
bool posctrl_calculate_yaw_and_rate_yaw(Position_ctrl* pos_ctrl);

// lean_angles_to_accel_xy - convert roll, pitch lean target angles to NE frame accelerations in cm/s/s
// todo: this should be based on thrust vector attitude control
void posctrl_lean_angles_to_accel_xy(Position_ctrl* pos_ctrl, float* accel_x_cmss, float* accel_y_cmss);


///
/// Vertical position controller
///

/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
///     speed_down can be positive or negative but will always be interpreted as a descent speed.
///     This function only needs to be called if using the kinematic shaping.
///     This can be done at any time as changes in these parameters are handled smoothly
///     by the kinematic shaping.
void posctrl_set_max_speed_accel_z(Position_ctrl* pos_ctrl, float speed_down, float speed_up, float accel_cmss);

/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
///     speed_down can be positive or negative but will always be interpreted as a descent speed.
///     This should be done only during initialisation to avoid discontinuities
void posctrl_set_correction_speed_accel_z(Position_ctrl* pos_ctrl, float speed_down, float speed_up, float accel_cmss);

/// get_max_accel_z_cmss - get the maximum vertical acceleration in cm/s/s
static inline float posctrl_get_max_accel_z_cmss(Position_ctrl* pos_ctrl) { return pos_ctrl->_accel_max_z_cmss; }

// get_pos_error_z_up_cm - get the maximum vertical position error up that will be allowed
static inline float posctrl_get_pos_error_z_up_cm(Position_ctrl* pos_ctrl) { return pos_ctrl->_p_pos_z._error_max; }

// get_pos_error_z_down_cm - get the maximum vertical position error down that will be allowed
static inline float posctrl_get_pos_error_z_down_cm(Position_ctrl* pos_ctrl) { return pos_ctrl->_p_pos_z._error_min; }

/// get_max_speed_up_cms - accessors for current maximum up speed in cm/s
static inline float posctrl_get_max_speed_up_cms(Position_ctrl* pos_ctrl) { return pos_ctrl->_vel_max_up_cms; }

/// get_max_speed_down_cms - accessors for current maximum down speed in cm/s.  Will be a negative number
static inline float posctrl_get_max_speed_down_cms(Position_ctrl* pos_ctrl) { return pos_ctrl->_vel_max_down_cms; }

/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
void posctrl_init_z_controller(Position_ctrl* pos_ctrl);

/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
///     This function does not allow any negative velocity or acceleration
void posctrl_init_z_controller_no_descent(Position_ctrl* pos_ctrl);

/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
///     This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
///     The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void posctrl_init_z_controller_stopping_point(Position_ctrl* pos_ctrl);

// relax_z_controller - initialise the position controller to the current position and velocity with decaying acceleration.
///     This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void posctrl_relax_z_controller(Position_ctrl* pos_ctrl, float throttle_setting);

/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
///     The vel is projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The function alters the vel to be the kinematic path based on accel
void posctrl_input_accel_z(Position_ctrl* pos_ctrl, const float accel);

/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_vel_accel_z(Position_ctrl* pos_ctrl, float *vel, const float accel, bool ignore_descent_limit, bool limit_output);

/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
///     using the default position control kinematic path.
///     ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
void posctrl_set_pos_target_z_from_climb_rate_cm(Position_ctrl* pos_ctrl, const float vel, bool ignore_descent_limit);

/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
///     The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The function alters the pos and vel to be the kinematic path based on accel
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_pos_vel_accel_z(Position_ctrl* pos_ctrl, float *pos, float *vel, const float accel, bool limit_output);

/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
///     using the default position control kinematic path.
void posctrl_set_alt_target_with_slew(Position_ctrl* pos_ctrl, const float pos);

/// update_pos_offset_z - updates the vertical offsets used by terrain following
void posctrl_update_pos_offset_z(Position_ctrl* pos_ctrl, float pos_offset_z);

/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
float posctrl_pos_offset_z_scaler(Position_ctrl* pos_ctrl,float pos_offset_z, float pos_offset_z_buffer);

// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool posctrl_is_active_z(Position_ctrl* pos_ctrl);

/// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors.
///     Position and velocity errors are converted to velocity and acceleration targets using PID objects
///     Desired velocity and accelerations are added to these corrections as they are calculated
///     Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
void posctrl_update_z_controller(Position_ctrl* pos_ctrl);


///
/// Accessors
///

/// set position, velocity and acceleration targets
void posctrl_set_pos_vel_accel(Position_ctrl* pos_ctrl, const Vector3p* pos, const Vector3f_t* vel, const Vector3f_t* accel);

/// set position, velocity and acceleration targets
void posctrl_set_pos_vel_accel_xy(Position_ctrl* pos_ctrl, const Vector2p* pos, const Vector2f_t* vel, const Vector2f_t* accel);


/// Position

/// set_pos_target_xy_cm - sets the position target in NEU cm from home
static inline void posctrl_set_pos_target_xy_cm(Position_ctrl* pos_ctrl, float pos_x, float pos_y) { pos_ctrl->_pos_target.x = pos_x; pos_ctrl->_pos_target.y = pos_y; }

/// get_pos_target_cm - returns the position target in NEU cm from home
static inline Vector3p posctrl_get_pos_target_cm(Position_ctrl* pos_ctrl) { return pos_ctrl->_pos_target; }

/// set_pos_target_z_cm - set altitude target in cm above home
static inline void posctrl_set_pos_target_z_cm(Position_ctrl* pos_ctrl, float pos_target) { pos_ctrl->_pos_target.z = pos_target; }

/// get_pos_target_z_cm - get target altitude (in cm above home)
static inline float posctrl_get_pos_target_z_cm(Position_ctrl* pos_ctrl) { return pos_ctrl->_pos_target.z; }

/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
///    function does not change the z axis
void posctrl_get_stopping_point_xy_cm(Position_ctrl* pos_ctrl, Vector2p *stopping_point);

/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void posctrl_get_stopping_point_z_cm(Position_ctrl* pos_ctrl, postype_t *stopping_point);

/// get_pos_error_cm - get position error vector between the current and target position
static inline Vector3f_t posctrl_get_pos_error_cm(Position_ctrl* pos_ctrl) { return vec3_sub2(&pos_ctrl->_pos_target, &pos_ctrl->_ahrs->relpos_cm); }

/// get_pos_error_xy_cm - get the length of the position error vector in the xy plane
static inline float posctrl_get_pos_error_xy_cm(Position_ctrl* pos_ctrl) { return math_norm(pos_ctrl->_pos_target.x - pos_ctrl->_ahrs->relpos_cm.x, pos_ctrl->_pos_target.y - pos_ctrl->_ahrs->relpos_cm.y); }

/// get_pos_error_z_cm - returns altitude error in cm
static inline float posctrl_get_pos_error_z_cm(Position_ctrl* pos_ctrl) { return (pos_ctrl->_pos_target.z - pos_ctrl->_ahrs->relpos_cm.z); }

/// Velocity

/// set_vel_desired_cms - sets desired velocity in NEU cm/s
static inline void posctrl_set_vel_desired_cms(Position_ctrl* pos_ctrl, const Vector3f_t *des_vel) { pos_ctrl->_vel_desired = *des_vel; }

/// set_vel_desired_xy_cms - sets horizontal desired velocity in NEU cm/s
static inline void posctrl_set_vel_desired_xy_cms(Position_ctrl* pos_ctrl, const Vector2f_t *vel) {pos_ctrl->_vel_desired.x = vel->x; pos_ctrl->_vel_desired.y = vel->y;}

/// get_vel_desired_cms - returns desired velocity (i.e. feed forward) in cm/s in NEU
static inline Vector3f_t posctrl_get_vel_desired_cms(Position_ctrl* pos_ctrl) { return pos_ctrl->_vel_desired; }

// get_vel_target_cms - returns the target velocity in NEU cm/s
static inline Vector3f_t posctrl_get_vel_target_cms(Position_ctrl* pos_ctrl) { return pos_ctrl->_vel_target; }

/// set_vel_desired_z_cms - sets desired velocity in cm/s in z axis
static inline void posctrl_set_vel_desired_z_cms(Position_ctrl* pos_ctrl, float vel_z_cms) {pos_ctrl->_vel_desired.z = vel_z_cms;}

/// get_vel_target_z_cms - returns current vertical speed in cm/s
static inline float posctrl_get_vel_target_z_cms(Position_ctrl* pos_ctrl) { return pos_ctrl->_vel_target.z; }


/// Acceleration

// set_accel_desired_xy_cmss set desired acceleration in cm/s in xy axis
static inline void posctrl_set_accel_desired_xy_cmss(Position_ctrl* pos_ctrl, const Vector2f_t *accel_cms)
{ 
    pos_ctrl->_accel_desired.x = accel_cms->x;
    pos_ctrl->_accel_desired.y = accel_cms->y;
}

// get_accel_target_cmss - returns the target acceleration in NEU cm/s/s
static inline Vector3f_t posctrl_get_accel_target_cmss(Position_ctrl* pos_ctrl) { return pos_ctrl->_accel_target; }


/// Offset

/// set_pos_offset_z_cm - set altitude offset in cm above home
static inline void posctrl_set_pos_offset_z_cm(Position_ctrl* pos_ctrl, float pos_offset_z) { pos_ctrl->_pos_offset_z = pos_offset_z; }

/// get_pos_offset_z_cm - returns altitude offset in cm above home
static inline float posctrl_get_pos_offset_z_cm(Position_ctrl* pos_ctrl) { return pos_ctrl->_pos_offset_z; }

/// get_vel_offset_z_cm - returns current vertical offset speed in cm/s
static inline float posctrl_get_vel_offset_z_cms(Position_ctrl* pos_ctrl) { return pos_ctrl->_vel_offset_z; }

/// get_accel_offset_z_cm - returns current vertical offset acceleration in cm/s/s
static inline float posctrl_get_accel_offset_z_cmss(Position_ctrl* pos_ctrl) { return pos_ctrl->_accel_offset_z; }


/// Outputs

/// get desired roll and pitch to be passed to the attitude controller
static inline float posctrl_get_roll_cd(Position_ctrl* pos_ctrl) { return pos_ctrl->_roll_target; }
static inline float posctrl_get_pitch_cd(Position_ctrl* pos_ctrl) { return pos_ctrl->_pitch_target; }

/// get desired yaw to be passed to the attitude controller
static inline float posctrl_get_yaw_cd(Position_ctrl* pos_ctrl) { return pos_ctrl->_yaw_target; }

/// get desired yaw rate to be passed to the attitude controller
static inline float posctrl_get_yaw_rate_cds(Position_ctrl* pos_ctrl) { return pos_ctrl->_yaw_rate_target; }

/// get desired roll and pitch to be passed to the attitude controller
Vector3f_t posctrl_get_thrust_vector(Position_ctrl* pos_ctrl);

/// get_bearing_to_target_cd - get bearing to target position in centi-degrees
int32_t posctrl_get_bearing_to_target_cd(Position_ctrl* pos_ctrl);

/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float posctrl_get_lean_angle_max_cd(Position_ctrl* pos_ctrl);

/// Other

/// get pid controllers
static inline P_2d_ctrl* posctrl_get_pos_xy_p(Position_ctrl* pos_ctrl) { return &pos_ctrl->_p_pos_xy; }
static inline P_1d_ctrl* posctrl_get_pos_z_p(Position_ctrl* pos_ctrl) { return &pos_ctrl->_p_pos_z; }
static inline PID_2d_ctrl* posctrl_get_vel_xy_pid(Position_ctrl* pos_ctrl) { return &pos_ctrl->_pid_vel_xy; }
static inline PID_basic_ctrl* posctrl_get_vel_z_pid(Position_ctrl* pos_ctrl) { return &pos_ctrl->_pid_vel_z; }
static inline PID_ctrl* posctrl_get_accel_z_pid(Position_ctrl* pos_ctrl) { return &pos_ctrl->_pid_accel_z; }

/// set_limit_accel_xy - mark that accel has been limited
///     this prevents integrator buildup
static inline void posctrl_set_externally_limited_xy(Position_ctrl* pos_ctrl) { pos_ctrl->_limit_vector.x = pos_ctrl->_accel_target.x; pos_ctrl->_limit_vector.y = pos_ctrl->_accel_target.y; }

// overrides the velocity process variable for one timestep
static inline void posctrl_override_vehicle_velocity_xy(Position_ctrl* pos_ctrl, const Vector2f_t* vel_xy) { pos_ctrl->_vehicle_horiz_vel = *vel_xy; pos_ctrl->_flags.vehicle_horiz_vel_override = true; }

// get_lean_angles_to_accel - convert roll, pitch lean target angles to lat/lon frame accelerations in cm/s/s
Vector3f_t posctrl_lean_angles_to_accel(const Vector3f_t* att_target_euler);

// enable or disable high vibration compensation
static inline void posctrl_set_vibe_comp(Position_ctrl* pos_ctrl, bool on_off) { pos_ctrl->_vibe_comp_enabled = on_off; }

/// get_vel_z_error_ratio - returns the proportion of error relative to the maximum request
static inline float posctrl_get_vel_z_control_ratio(Position_ctrl* pos_ctrl) { return math_constrain_float(pos_ctrl->_vel_z_control_ratio, 0.0f, 1.0f); }

/// crosstrack_error - returns horizontal error to the closest point to the current track
float posctrl_crosstrack_error(Position_ctrl* pos_ctrl);

/// standby_xyz_reset - resets I terms and removes position error
///     This function will let Loiter and Alt Hold continue to operate
///     in the event that the flight controller is in control of the
///     aircraft when in standby.
void posctrl_standby_xyz_reset(Position_ctrl* pos_ctrl);

//
void posctrl_publish_msg_xy(Position_ctrl* pos_ctrl);
void posctrl_publish_msg_z(Position_ctrl* pos_ctrl);

//
void posctrl_assign_param(Position_ctrl* pos_ctrl);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



